/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "JCMaCIJointGroupControl.h"
#include <assert.h>

JCMaCIJointGroupControl::JCMaCIJointGroupControl(gimi::GIMI *aGIMIPtr,
						 wxSizer *compSizer,
						 wxPanel *aComponentPanel)
  : JoystickControlComponent (aGIMIPtr, compSizer, aComponentPanel, MJGC_NAME,
                              GIMI_PROTOCOL_MACI_JOINTGROUPCTRL, -1),
    iJointClient(NULL),
    iNumberOfJoints (0),
    iPositionControlledJointsSpinCtrl(),
    iCommandThread (),
    iTarget ()
{
  dPrint(1,"MaCI Joint Group Control constructor called!");
  configOptions.addButtonOption(MJGC_STOP_BUTTON , "Deadman switch");
  configOptions.addButtonOption(MJGC_SPEED_BUTTON , "Full speed button");
}

JCMaCIJointGroupControl::~JCMaCIJointGroupControl()
{
  void *param;
  // Stop the commandsenderthread gracefully.
  activationStatus.deactivate();
  if (iCommandThread != NULL) {
    ownThread_Join(iCommandThread, &param);
    iCommandThread = NULL;
  }
  //Delete all wxSpinCtrl from map....USE THE ITERATOR
  for(std::map<std::string,wxSpinCtrl *>::iterator i = iPositionControlledJointsSpinCtrl.begin(); 
      i != iPositionControlledJointsSpinCtrl.end(); i++){
    delete(i->second);

  }
}
    
bool JCMaCIJointGroupControl::startComponent()
{
  bool result = false;

  dPrint(1, "Activation status %d", activationStatus.isActive());
  if (activationStatus.isActive()) {
    dPrint(2, "Component MaCIJointGroupControl is already activated!");
    result = false;
  } else {
    dPrint(2, "Starting component MaCIJointGroupControl");
    
    if (!iTarget.clientName.empty()) {
     
      assert(iGIMIPtr != NULL);
      iJointClient = new MaCI::JointGroupCtrl::CJointGroupCtrlClient(iGIMIPtr,iTarget.minorServiceId);
      if (iJointClient != NULL){

	iJointClient->SetDefaultTarget(iTarget.clientName);
	if(iJointClient->Open() == MaCI::KMaCIOK)
        {
	  MaCI::JointGroupCtrl::TJointGroupInformation  jointInfo;
          if (iJointClient->GetJointGroupInformation(jointInfo,1000)) {
            // Configure axes.
            iNumberOfJoints = jointInfo.size();
            int axisId = 0;
  
            //// Add joints to axes.
            for (MaCI::JointGroupCtrl::TJointGroupInformation::const_iterator
                 jointIter = jointInfo.begin(); jointIter != jointInfo.end(); ++jointIter)
            {
              configOptions.addAxisOption(axisId, jointIter->jointName);
              ++axisId;
            }
            
            //// Draw component information.
            wxFlexGridSizer* fgSizer1;
            fgSizer1 = new wxFlexGridSizer( 2, 2, 0, 0 );
            fgSizer1->SetFlexibleDirection( wxVERTICAL );
            fgSizer1->SetNonFlexibleGrowMode( wxFLEX_GROWMODE_SPECIFIED );
           
            wxStaticText *compNameText = new wxStaticText(componentPanel, wxID_ANY,
                wxT("JointControl"), wxDefaultPosition, wxDefaultSize, 0);
            wxStaticText *compActText = new wxStaticText(componentPanel, wxID_ANY,
                wxT("Active"), wxDefaultPosition, wxDefaultSize, 0);
            wxStaticText *empty = new wxStaticText(componentPanel, wxID_ANY,
                wxT(""), wxDefaultPosition, wxDefaultSize, 0);
            fgSizer1->Add(compNameText, 1 ,wxALL, 5);
            fgSizer1->Add(compActText, 1, wxALL, 5);
            fgSizer1->Add(empty, 1, wxALL, 5);
            fgSizer1->Add(empty, 1, wxALL, 5);
            
            bool firstPositionControlledJoint = true;
            for(MaCI::JointGroupCtrl::TJointGroupInformation::const_iterator
                 jointIter = jointInfo.begin(); jointIter != jointInfo.end(); ++jointIter)
            {
             
              if(jointIter->canControlPosition ){
               
                if(firstPositionControlledJoint){
                  firstPositionControlledJoint = false;
                  wxStaticText *jointName = new wxStaticText(componentPanel, wxID_ANY,
                                                                wxT("Position controlled joint"), wxDefaultPosition, wxDefaultSize, 0);
                  wxStaticText *speedValueText = new wxStaticText(componentPanel, wxID_ANY,
                                                               wxT("Speed (max = 30)"), wxDefaultPosition, wxDefaultSize, 0);
                  fgSizer1->Add(jointName, 1, wxALL, 5);
                  fgSizer1->Add(speedValueText, 1, wxALL, 5);
                }
                  
                wxStaticText *jointNameText = new wxStaticText(componentPanel, wxID_ANY,
                                                               wxT(jointIter->jointName), wxDefaultPosition, wxDefaultSize, 0);
                wxSpinCtrl *speedValue = new wxSpinCtrl(componentPanel, wxID_ANY, wxT("5"), wxDefaultPosition,wxDefaultSize, wxSP_ARROW_KEYS,1,30);
                
                iPositionControlledJointsSpinCtrl[std::string(jointIter->jointName)] =  speedValue;
                fgSizer1->Add(jointNameText, 1, wxALL, 5);
                fgSizer1->Add(speedValue, 1, wxALL, 5);

               
              }

            }
            
            componentSizer->Add(fgSizer1, 1, wxEXPAND, 5);
            
            //// Activate
            activationStatus.activate();
            iCommandThread = ownThread_Create((void *)(commandSenderThread), this);
            result = true;
            dPrint(1, "MaCIJointGroupControl activated.");
            dPrint(1, "Activation status is %d", activationStatus.isActive());
          } else {
            dPrint(1, "Failed to get joint-information from MaCI JointGroupCtrl-client");
          }
        } else { // Failed to initialize JointSpeedControl
          dPrint(1, "Failed to Open MaCI JointGroupCtrl-client");
        }

    
      }else{
	dPrint(1,"Failed to Create jointGroupCtrl client");
      }
    
    
    } else { // No targets.
      dPrint(1, "Cannot start component without targets.");
    }
    
  }
  return result;
}
    
int JCMaCIJointGroupControl::addTarget(const gimi::GIMIService &aTarget)
{
  dPrint(1,"add target calles");
  iTarget = aTarget;
  return JOYSTICKCONTROL_OK;
}

void JCMaCIJointGroupControl::commandSenderThread(void *p)
{
  dPrint(1, "Started");
  
  JCMaCIJointGroupControl *control =
      static_cast<JCMaCIJointGroupControl *>(p);
  
  gim::time current, nextrun, timediff(50);

  bool brakeState = true; ///< True for brakes on, false for brakes off.
  bool driveStateChanged = false; ///< Send new setSpeed if brakes have
                                  ///< changed state (even if axes have not)
    float deadzone = 0.05;
  
  MaCI::JointGroupCtrl::TJointGroupInformation  jointInfo;
  dPrint(1,"Trying to get joint info");
  while(!control->iJointClient->GetJointGroupInformation(jointInfo,1000)){
    dPrint(1,"Still trying to get joint info");
  }
  dPrint(1,"Got information about joints");
  //Getting all the joints position.(If they can be controlled by position commands,
  //if not adding 0)
  std::vector<float> currentPos;
  for (int currentAxis = 0; currentAxis < control->iNumberOfJoints; ++currentAxis) {
    if(jointInfo[currentAxis].canControlPosition){
      float position;
      if(control->iJointClient->GetPosition(currentAxis,position,1000)){
	currentPos.push_back(position);
      }
    }else{
      currentPos.push_back(0.0);
    }
  }
  bool fullSpeed = true;
  bool fullSpeedStateChanged = false;
  // Send current status at some interval, if status has been changed.
  // Send changes in buttons immediately (brakes and gears)

  while (control->activationStatus.isActive()) {
  
    //// Buttons
    bool buttonDown;

    // Deadman switch
    if (control->configOptions.readButtonOption(MJGC_STOP_BUTTON, &buttonDown) ==
        JOYSTICKCONTROL_STATUS_CHANGED)
    {
      dPrint(1, "Unlock joints button down.");
      if (buttonDown) {
        dPrint(1, "Deadman switch is down.");
        brakeState = false;
        driveStateChanged = true;
      } else {
        dPrint(1, "Deadman switch is up.");
        brakeState = true;
        driveStateChanged = true;
      }
    }
    
    if (control->configOptions.readButtonOption(MJGC_SPEED_BUTTON, &buttonDown) ==
        JOYSTICKCONTROL_STATUS_CHANGED)
    {
      dPrint(1,"FULL SPEED BUTTON PRESSED %d",!fullSpeed);
      fullSpeed = !fullSpeed;
      fullSpeedStateChanged = true;
    }

    //// Send axis events every ~50ms
    current.setToCurrent();
    if (current > nextrun) {
      nextrun = current + timediff;
      float axisValue;

      //// Loop axes and send new state from each if changed.
      for (int currentAxis = 0; currentAxis < control->iNumberOfJoints; ++currentAxis) {
        int axisChanged = control->configOptions.readAxisOption(currentAxis, &axisValue);
        dPrint(100,"axisChanged %d, JOYSTICKCONTROL_STATUS_CHANGED %d",axisChanged, JOYSTICKCONTROL_STATUS_CHANGED);
        if (jointInfo[currentAxis].canControlPosition || 
	    axisChanged == JOYSTICKCONTROL_STATUS_CHANGED || 
	    driveStateChanged||
            fullSpeedStateChanged) {
          if (!brakeState) {
            // Adjust curve. (^2 for smoother turning)
	   
            float expAdjustedAxis;
            /*
            if (abs(axisValue) > deadzone) {
              
            }
            */
	    if(jointInfo[currentAxis].canControlSpeed){
	      //Controlling using SPEED commands
	      if (axisValue < deadzone) {
                //Added 0.5, because otherwise it's too fast, at least for Avant ;)
       
		expAdjustedAxis = (-axisValue * axisValue)*jointInfo[currentAxis].maxSpeed;
	      } else if(axisValue > deadzone){
		expAdjustedAxis = axisValue * axisValue*jointInfo[currentAxis].maxSpeed;
	      }else{
		expAdjustedAxis = 0;
	      }
              if(!fullSpeed){
                expAdjustedAxis = expAdjustedAxis*0.5;
              }
	      dPrint(1, "Set speed %.2f for axis %d.", expAdjustedAxis, currentAxis);
	      control->iJointClient->SetSpeed(expAdjustedAxis,currentAxis);
	      driveStateChanged = false;
	    }else if(jointInfo[currentAxis].canControlPosition){
	      //Controlling using POSITION commands
              //First get current position
              /* for (int cAxis = 0; cAxis < control->iNumberOfJoints; ++cAxis) {

              if(jointInfo[cAxis].canControlPosition){
                  float position;
                  if(control->iJointClient->GetPosition(cAxis,position,1000)){
                    dPrint(10,"Got position for axis %d, position %f",cAxis,position);
                   currentPos[cAxis] = position;
                  }
                }else{
		  currentPos[cAxis] = 0.0;
                }
                }*/
            
              float position;
              if(control->iJointClient->GetPosition(currentAxis,position,1000)){
                dPrint(10,"Got position for axis %d, position %f",currentAxis,position);
                currentPos[currentAxis] = position;
              
              }else{
                currentPos[currentAxis] = 0.0;
              }
              //Then calculate new position
	      dPrint(10,"CurrentPos %f, axis %d, axisValue %f", currentPos[currentAxis],currentAxis,axisValue);
           
              //Searching the divider(speed) from GUI
              int speedValue = (control->iPositionControlledJointsSpinCtrl[std::string(jointInfo[currentAxis].jointName)])->GetValue();

	      float positionStep = (jointInfo[currentAxis].maxPosition -jointInfo[currentAxis].minPosition)/100*speedValue;
	      if (axisValue < -deadzone) {
		expAdjustedAxis = currentPos[currentAxis] + (-axisValue * axisValue)*positionStep;
	      } else if(axisValue > deadzone){
		expAdjustedAxis = currentPos[currentAxis] + axisValue * axisValue*positionStep;
	      } else {
		expAdjustedAxis = currentPos[currentAxis];
                dPrint(10,"movement smaller than the deadzone");
	      }
	      if(expAdjustedAxis > jointInfo[currentAxis].maxPosition){
		expAdjustedAxis = jointInfo[currentAxis].maxPosition;
	      }else if(expAdjustedAxis < jointInfo[currentAxis].minPosition){
		expAdjustedAxis = jointInfo[currentAxis].minPosition;
	      }

              //	      currentPos[currentAxis] = expAdjustedAxis;
	      dPrint(10, "Set pos %.2f for axis %d.positionStep %f", expAdjustedAxis, currentAxis,positionStep);
	      control->iJointClient->SetPosition(expAdjustedAxis,currentAxis);

	      driveStateChanged = false;
	    }

			
	  } else { // Lock set
	    if(jointInfo[currentAxis].canControlSpeed){
	      control->iJointClient->SetSpeed(0,currentAxis);
	    }
	    driveStateChanged = false;
	  }
	  
        }
      }
      if(fullSpeedStateChanged){
        fullSpeedStateChanged = false;
      }
    }
    ownSleep_ms(5);
  }
  
  dPrint(1, "Stopping");
  
  return;
}

